#include "CSerialPort.h"
#include "Tracker.h"
#include <opencv2/videoio.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <iomanip>
#include <termios.h>
#include <netinet/in.h>
#include<sys/ioctl.h>
#include <thread>
using namespace std;
using namespace cv;

const int IMG_WIDTH = 640, IMG_HEIGHT =480;

vector<Point2f> center;
CSerialPort myserialport; //打开串口
 DataStruct send_data = {0, 0, 0, 0, 0xFF};

 
int main(int argc, char **argv)
{
	CommandLineParser parser(argc, argv, "{@input_path |0|input path can be a camera id, like 0,1,2 or a video filename}");
	parser.printMessage();
	string input_path = parser.get<string>(0);
	string video_name = input_path;
	VideoCapture video_in;
	if ((isdigit(input_path[0]) && input_path.size() == 1))
	{
		int camera_no = input_path[0] - '0';
		video_in.open(camera_no);
	}
	else
	{
		video_in.open(video_name);
	}
	if (!video_in.isOpened())
	{
		cerr << "Couldn't open " << video_name << endl;
		return 1;
	}

	center.push_back(Point2f(IMG_WIDTH / 2, IMG_HEIGHT / 2));

	Ptr<AKAZE> akaze = AKAZE::create();
	akaze->setThreshold(akaze_thresh);
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	Tracker akaze_tracker(akaze, matcher);
	Mat frame;
	namedWindow(video_name, WINDOW_NORMAL);

	cout << "\nPress any key to stop the video and select a bounding box" << endl;
	while (waitKey(1) < 1)
	{
		video_in >> frame;
		cv::resizeWindow(video_name, frame.size());
		imshow(video_name, frame);
	}
	//vector<Point2f> bb;
	cv::Rect uBox = cv::selectROI(video_name, frame);

	akaze_tracker.setFirstFrame(frame, uBox);

	Mat akaze_res;
	double Time;

	while (1)
	{	
		Time = (double)cvGetTickCount();
		send_data[0]=0;
		cout << "\r\n----------------------------------" << endl;
		video_in >> frame;
		if (frame.empty())
		{
			try
			{
				video_in.open(0);
			}
			catch (Exception e)
			{
				cout << e.msg << endl;
			}
			cout << "Camera LOST!" << endl;
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			continue;
		}

		Mat homography = akaze_tracker.process(frame);
		cv::imshow(video_name, frame);
		if (waitKey(1) == 27)
			break;
		if (homography.empty())
		{
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			 continue;
		}
		vector<Point2f> new_center;
		perspectiveTransform(center, new_center, homography);
		short x = doubleToShort(new_center[0].x - 320.0d);
		
		cout<< "\r\nx:\t" << x/3 << endl;
		send_data[0]=2;
		send_data[5] = x;
		send_data[6] = (x >> 8);
		send_data[7] = 0x5A;
		myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
		Time = (double)cvGetTickCount() - Time;                         //显示时间，单位：ms
		Time = Time / (cvGetTickFrequency() * 1000);
		cout<<"Time:"<<Time<<endl;
	}
	return 0;
}
